#ifndef KEYPOSEFRAME_H
#define KEYPOSEFRAME_H

#include "PoseFrame.h"
#include "Map.h"
//#include "KeyFrameDatabase.h"
#include "ImuTypes.h"

#include <mutex>

#include <boost/serialization/base_object.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>

#include <set>

using namespace std;
namespace msf
{
class Map;
class PoseFrame;

class KeyPoseFrame
{
    friend class boost::serialization::access;

    template<class Archive>
    void serialize(Archive& ar, const unsigned int version)
    {
        ar & mnId;
        ar & const_cast<long unsigned int&>(mnFrameId);
        ar & const_cast<double&>(mTimeStamp);
      
        // Variables of tracking
        //ar & mnTrackReferenceForFrame;
        //ar & mnFuseTargetForKF;
        // Variables of local mapping
        //ar & mnBALocalForKF;
        //ar & mnBAFixedForKF;
        //ar & mnNumberOfOpt;
        // Variables used by KeyFrameDatabase
        //ar & mnLoopQuery;
        //ar & mnLoopWords;
        //ar & mLoopScore;
        //ar & mnRelocQuery;
        //ar & mnRelocWords;
        //ar & mRelocScore;
        //ar & mnMergeQuery;
        //ar & mnMergeWords;
        //ar & mMergeScore;
        //ar & mnPlaceRecognitionQuery;
        //ar & mnPlaceRecognitionWords;
        //ar & mPlaceRecognitionScore;
        //ar & mbCurrentPlaceRecognition;
        // Variables of loop closing
        //serializeMatrix(ar,mTcwGBA,version);
        //serializeMatrix(ar,mTcwBefGBA,version);
        //serializeMatrix(ar,mVwbGBA,version);
        //serializeMatrix(ar,mVwbBefGBA,version);
        //ar & mBiasGBA;
        //ar & mnBAGlobalForKF;
        // Variables of Merging
        //serializeMatrix(ar,mTcwMerge,version);
        //serializeMatrix(ar,mTcwBefMerge,version);
        //serializeMatrix(ar,mTwcBefMerge,version);
        //serializeMatrix(ar,mVwbMerge,version);
        //serializeMatrix(ar,mVwbBefMerge,version);
        //ar & mBiasMerge;
        //ar & mnMergeCorrectedForKF;
        //ar & mnMergeForKF;
        //ar & mfScaleMerge;
        //ar & mnBALocalForMerge;

        // Pose relative to parent
        //serializeSophusSE3<Archive>(ar, mTcp, version);

        // Pose
        //serializeSophusSE3<Archive>(ar, mTow, version);

        // Connected KeyFrameWeight
        ar & mBackupConnectedKeyPoseFrameIdWeights;
        // Spanning Tree and Loop Edges
        ar & mbFirstConnection;
        ar & mBackupParentId;
        ar & mvBackupChildrensId;
        ar & mvBackupLoopEdgesId;
        ar & mvBackupMergeEdgesId;
        // Bad flags
        ar & mbNotErase;
        ar & mbToBeErased;
        ar & mbBad;

        ar & mHalfBaseline;

        ar & mnOriginMapId;

        // Inertial variables
        ar & mImuBias;
        ar & mBackupImuPreintegrated;
        ar & mImuCalib;
        ar & mBackupPrevKPFId;
        ar & mBackupNextKPFId;
        ar & bImu;
        ar & boost::serialization::make_array(mVw.data(), mVw.size());
        ar & boost::serialization::make_array(mOwb.data(), mOwb.size());
        ar & mbHasVelocity;
    }

public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      KeyPoseFrame();
      KeyPoseFrame(PoseFrame &F, Map *pMap);

      // Pose functions
      void SetPose(const Sophus::SE3f &Tcw);
      void SetVelocity(const Eigen::Vector3f &Vw_);

      Sophus::SE3f GetPose();

      Sophus::SE3f GetPoseInverse();
      Eigen::Vector3f GetOdomCenter();

      Eigen::Vector3f GetImuPosition();
      Eigen::Matrix3f GetImuRotation();
      Sophus::SE3f GetImuPose();
      Eigen::Matrix3f GetRotation();
      Eigen::Vector3f GetTranslation();
      Eigen::Vector3f GetVelocity();
      bool isVelocitySet();

      // Covisibility graph functions
      void AddConnection(KeyPoseFrame* pKPF, const int &weight);
      void EraseConnection(KeyPoseFrame* pKPF);

      void UpdateConnections(bool upParent=true);
      void UpdateBestCovisibles();
      std::set<KeyPoseFrame *> GetConnectedKeyPoseFrames();
      std::vector<KeyPoseFrame* > GetVectorCovisibleKeyPoseFrames();
      std::vector<KeyPoseFrame*> GetBestCovisibilityKeyPoseFrames(const int &N);
      std::vector<KeyPoseFrame*> GetCovisiblesByWeight(const int &w);
      int GetWeight(KeyPoseFrame* pKPF);

      // Spanning tree functions
      void AddChild(KeyPoseFrame* pKPF);
      void EraseChild(KeyPoseFrame* pKPF);
      void ChangeParent(KeyPoseFrame* pKPF);
      std::set<KeyPoseFrame*> GetChilds();
      KeyPoseFrame* GetParent();
      bool hasChild(KeyPoseFrame* pKPF);
      void SetFirstConnection(bool bFirst);

      // Enable/Disable bad flag changes
      void SetNotErase();
      void SetErase();

      // Set/check bad flag
      void SetBadFlag();
      bool isBad();

      static bool weightComp( int a, int b){
            return a>b;
      }

      static bool lId(KeyPoseFrame* pKPF1, KeyPoseFrame* pKPF2){
            return pKPF1->mnId<pKPF2->mnId;
      } 

      Map* GetMap();
      void UpdateMap(Map* pMap);

      void SetNewBias(const IMU::Bias &b);
      Eigen::Vector3f GetGyroBias();

      Eigen::Vector3f GetAccBias();

      IMU::Bias GetImuBias();

      void PreSave(set<KeyPoseFrame*>& spKPF);

      bool bImu;

      // 以下变量仅从1个线程访问或从不更改（不需要互斥）
public:

      static long unsigned int nNextId;
      long unsigned int mnId;
      const long unsigned int mnFrameId;

      const double mTimeStamp;

      // tracking中使用的变量
      long unsigned int mnTrackReferenceForPoseFrame;
      long unsigned int mnFuseTargetForKPF;

      // local optimize中使用的变量
      long unsigned int mnBALocalForKPF;
      long unsigned int mnBAFixedForKPF;

      // BA优化数量(BA中迭代的次数)
      long unsigned int mnNumberOfOpt;

      // Variables used by the keyframe database
      
      // Pose relative to parent (this is computed when bad flag is activated)
      Sophus::SE3f mTop;

      // IMUinitialize中使用
      Sophus::SE3f mTowGBA;
      Sophus::SE3f mTowBefGBA;
      Eigen::Vector3f mVwbGBA;
      Eigen::Vector3f mVwbBefGBA;
      IMU::Bias mBiasGBA;
      long unsigned int mnBAGlobalForKPF;

      // 从先前关键位姿帧开始的IMU预积分测量值
      KeyPoseFrame* mPrevKPF;
      KeyPoseFrame* mNextKPF;

      IMU::Preintegrated* mpImuPreintegrated;
      IMU::Calib mImuCalib;

      unsigned int mnOriginMapId;


      //以下变量需要通过mutex来获得以保证线程安全
protected:
      //sophus poses
      Sophus::SE3<float> mTow;
      Eigen::Matrix3f mRow;
      Sophus::SE3<float> mTwo;
      Eigen::Matrix3f mRwo;

      // IMU position
      Eigen::Vector3f mOwb;
      // Velocity
      Eigen::Vector3f mVw;
      bool mbHasVelocity;

      // Imu bias
      IMU::Bias mImuBias;

      std::map<KeyPoseFrame*,int> mConnectedKeyPoseFrameWeights;
      std::vector<KeyPoseFrame*> mvpOrderedConnectedKeyPoseFrames;
      std::vector<int> mvOrderedWeights;
      // 对于没有指针的保存关系，这对于保存/加载函数是必要的
      std::map<long unsigned int, int> mBackupConnectedKeyPoseFrameIdWeights;

      // 生成树和回环边
      bool mbFirstConnection;
      KeyPoseFrame* mpParent;
      std::set<KeyPoseFrame*> mspChildrens;
      std::set<KeyPoseFrame*> mspLoopEdges;
      std::set<KeyPoseFrame*> mspMergeEdges;
      // For save relation without pointer, this is necessary for save/load function
      long long int mBackupParentId;
      std::vector<long unsigned int> mvBackupChildrensId;
      std::vector<long unsigned int> mvBackupLoopEdgesId;
      std::vector<long unsigned int> mvBackupMergeEdgesId;

      // Bad flags
      bool mbNotErase;
      bool mbToBeErased;
      bool mbBad;    

      float mHalfBaseline; // Only for visualization

      Map* mpMap;

      // Backup variables for inertial
      long long int mBackupPrevKPFId;
      long long int mBackupNextKPFId;
      IMU::Preintegrated mBackupImuPreintegrated;

      // Mutex
      std::mutex mMutexPose; // for pose, velocity and biases
      std::mutex mMutexConnections;
      std::mutex mMutexMap;
};
}

#endif // KEYPOSEFRAME_H